perm filename XLOU[GEM,BGB] blob
sn#030948 filedate 1973-03-25 generic text, type T, neo UTF8
00100 TITLE XLOU
00200
00300 EXTERN DPYSTR,DECDPY,CTRL,META,AIVECT,AVECT
00400 EXTERN DPYOUT,GEODPY,FLODPY,DPYSET,BDET,BATT
00500 EXTERN WORLD
00600
00700 DPYBUF: .+2
00800 =100
00900 BLOCK =100
00100 SUBR(XLOU)--------------------------------------------------------
00200 BEGIN XLOU; LOU PAUL ARM FILE SIMULATION - BGB - 21 FEBRUARY 1973.
00300
00400 SKIPE TICK↔GO L0
00500 ;SPECIFY FIRST TICK & DELTA TICK.
00600 BEGIN
00700 SETZ 1,↔SKIPN CTRL↔GO L2
00800 OUTSTR[ASCIZ/ FIRST TICK = /]
00900 L1: INCHWL↔CAIGE"0"↔GO L2↔CAILE"9"↔GO L2
01000 ANDI 17↔IMULI 1,12↔ADD 1,0↔GO L1
01100 L2: DAC 1,TOCK↔INCHRS↔JFCL
01200 SETZ 1,↔SKIPN CTRL↔GO L4
01300 OUTSTR[ASCIZ/ DELTA TICK = /]
01400 L3: INCHWL↔CAIGE"0"↔GO L4↔CAILE"9"↔GO L4
01500 ANDI 17↔IMULI 1,12↔ADD 1,0↔GO L3
01600 L4: SKIPN 1↔LACI 1,=10↔DAC 1,DELTIC↔INCHRS↔JFCL
01700 BEND
01800
01900 ;GET THE PIECES OF THE ARM.
02000 SETOM 1,TICK
02100 SETQ(J1,{BFETCH,{[[ASCII/JOINT1/]]}})
02200 SETQ(J2,{BFETCH,{[[ASCII/JOINT2/]]}})
02300 SETQ(J3,{BFETCH,{[[ASCII/JOINT3/]]}})
02400 SETQ(J4,{BFETCH,{[[ASCII/JOINT4/]]}})
02500 SETQ(J5,{BFETCH,{[[ASCII/JOINT5/]]}})
02600 SETQ(J6,{BFETCH,{[[ASCII/JOINT6/]]}})
02700 SETQ(F1,{BFETCH,{[[ASCIZ/FING1/]]}})
02800 SETQ(F2,{BFETCH,{[[ASCIZ/FING2/]]}})
02900 SETQ(BLK,{BFETCH,{[[ASCIZ/BLOCK/]]}})
03000
03100 ;AD HOC GET ARM TRAJECTORY FILE.
03200 INIT 2,10↔SIXBIT/DSK/↔IBUF↔HALT
03300 LAC[SIXBIT/OVER/]↔DAC FILNAM
03400 LAC[SIXBIT/LOU/]↔DAC FILNAM+1
03500 SETZM FILNAM+2↔SETZM FILNAM+3
03600 LOOKUP 2,FILNAM↔POP0J
03700 PUSH P,121↔LACI BUFFER↔DAC 121↔INBUF 2,↔POP P,121
03800
03900 ;READ HEADER WORD.
04000 L0: CALL(WORDIN)↔JUMPE EOX
04100 DAC 1↔ANDI 77↔ANDCMI 1,77
04200 CAMN 1,[SIXBIT/THETA/]↔GO L1
04300 CAMN 1,[SIXBIT/HAND/]↔GO L2
04400 CAMN 1,[SIXBIT/TICK/]↔GO L3
04500 CAMN 1,[SIXBIT/NEXT/]↔GO L4
04600 CAMN 1,[SIXBIT/DAC/]↔GO L5
04700 OUTSTR[ASCIZ/ LABEL NOT KNOW.
04800 /]↔ GO EOX
04900
00100 ;THETA CONTROL BLOCK.
00200 L1: CALL(WORDIN)↔DAC J6NEW
00300 CALL(WORDIN)↔DAC J5NEW
00400 CALL(WORDIN)↔DAC J4NEW
00500 CALL(WORDIN)↔DAC J3NEW
00600 CALL(WORDIN)↔DAC J2NEW
00700 CALL(WORDIN)↔DAC J1NEW
00800 GO L0
00900
01000 ;HAND CONTROL BLOCK.
01100 L2: CALL(WORDIN)↔DAC J7NEW
01200 GO L0
01300
00100 ;TICK BLOCK.
00200 L3: CALL(WORDIN)
00300 AOS TICK
00400 CALL(DPYSET,DPYBUF)
00500
00600 CALL(AIVECT,[=400],[=500-=150])↔CALL(DECDPY,TICK)
00700 CALL(DPYSTR,{[[ASCIZ/ TICKS/]]})
00800 CALL(DPYOUT,[13])
00900 LAC TICK↔JUMPE M9 ;FIRST TIME THRU.
01000 IDIV DELTIC↔SKIPE 1↔GO L0
01100
01200 ;DISPLAY ARM POSITION NUMERICALLY.
01300 CALL(DPYSET,DPYBUF)
01400 CALL(AIVECT,[=400],[=460-=150])↔CALL(FLODPY,J1NEW,[1])
01500 CALL(AIVECT,[=400],[=440-=150])↔CALL(FLODPY,J2NEW,[1])
01600 CALL(AIVECT,[=400],[=420-=150])↔CALL(FLODPY,J3NEW,[1])
01700
01800 CALL(AIVECT,[=400],[=380-=150])↔CALL(FLODPY,J4NEW,[1])
01900 CALL(AIVECT,[=400],[=360-=150])↔CALL(FLODPY,J5NEW,[1])
02000 CALL(AIVECT,[=400],[=340-=150])↔CALL(FLODPY,J6NEW,[1])
02100
02200 CALL(AIVECT,[=400],[=300-=150])↔CALL(FLODPY,J7NEW,[=3])
02300 CALL(DPYOUT,[14])
02400
00100 ;MOVE THE PIECES OF THE ARM TO THE NEW POSITION.
00200 EXTERN ROTATE,TRANSLATE
00300 M1: ;SHOULDER JOINT 1.
00400 SKIPN 1,J1↔GO M2
00500 LAC 2,J1NEW↔FSBR 2,J1OLD
00600 FMPR 2,[1745.32925E-5]↔DAC 2,DELTA
00800 SETZ↔LACN 1,J1↔CALL(ROTATE,1,0,0,DELTA)
01000 M2: ;ELEVATION JOINT 2.
01100 SKIPN 1,J2↔GO M3
01200 LAC 2,J2NEW↔FSBR 2,J2OLD
01300 FMPR 2,[1745.32925E-5]↔DACN 2,DELTA
01400 SETZ↔LACN 1,J2↔CALL(ROTATE,1,0,0,DELTA)
01700 M3: ;SLIDE JOINT 3.
01800 SKIPN 1,J3↔GO M4
01900 LAC 2,J3NEW↔FSBR 2,J3OLD
02000 FDVR 2,[12.0]↔DAC 2,DELTA
02100 SETZ↔LACN 1,J3↔CALL(TRANSLATE,1,0,0,DELTA)
00100 M4: ;FORE ARM ROTATION.
00200 SKIPN 1,J4↔GO M5
00300 LAC 2,J4NEW↔FSBR 2,J4OLD
00400 FMPR 2,[1745.32925E-5]↔DAC 2,DELTA
00500 SETZ↔LACN 1,J4↔CALL(ROTATE,1,0,DELTA,0)
00800 M5: ;WRIST FLAP.
00900 SKIPN 1,J5↔GO M6
01000 LAC 2,J5NEW↔FSBR 2,J5OLD
01100 FMPR 2,[1745.32925E-5]↔DAC 2,DELTA
01200 SETZ↔LACN 1,J5↔CALL(ROTATE,1,0,0,DELTA)
01500 M6: ;WRIST ROTATION.
01600 SKIPN 1,J6↔GO M7
01700 LAC 2,J6NEW↔FSBR 2,J6OLD
01800 FMPR 2,[1745.32925E-5]↔DAC 2,DELTA
01900 SETZ↔LACN 1,J6↔CALL(ROTATE,1,0,0,DELTA)
00100 M7: ;FINGERS.
00200 LAC 2,J7NEW↔FSBR 2,J7OLD
00300 FDVR 2,[24.0]↔DAC 2,DELTA
00500 SKIPN 1,F1↔GO M8
00600 SETZ↔LACN 1,F1↔CALL(TRANSLATE,1,0,0,DELTA)
00900 M8:
01000 SKIPN 1,F2↔GO M9
01100 SETZ↔LACN 1,F2↔CALL(TRANSLATE,1,0,0,DELTA)
01400
01500 SKIPN 1,BLK↔GO M10↔LAC 1,J6
01600 LAC 2,GRASP#↔LAC J7NEW
01700 CAMGE[1.10]↔JUMPE 2,[CALL(BATT,BLK,1)↔SETOM GRASP↔GO .+3]
01800 CAMLE[1.10]↔JUMPN 2,[CALL(BDET,BLK)↔SETZM GRASP↔GO .+1]
00100 M10: LAC TICK↔CAMGE TOCK↔GO M9
00200 CALL(GEODPY)
00300 ;UPDATE OLD TO NEW.
00400 M9: LAC[XWD J1NEW,J1OLD]↔BLT J7OLD
00500 LAC TICK↔JUMPE L0
00600 INCHRS↔SKIPE META↔GO[OUTSTR[ASCIZ/...
00700 */]↔POP0J]
00800 GO L0
00100 ;NEXT BLOCK.
00200 L4: CALL(WORDIN)↔DAC NEXT#
00300 GO L0
00400
00500 ;DAC BLOCK.
00600 L5: CALL(WORDIN)↔CALL(WORDIN)↔CALL(WORDIN)
00700 CALL(WORDIN)↔CALL(WORDIN)↔CALL(WORDIN)↔GO L0
00800
00900 ;END OF FILE.
01000 EOX: SETZM TICK
01100 RELEASE 2,↔OUTSTR[ASCIZ/ EOF
01200 */]↔ POP0J
01300
01400 FILNAM: BLOCK 4
01500 IBUF: BLOCK 3
01600 BUFFER: BLOCK 410
01700
01800 DECLARE{J1,J2,J3,J4,J5,J6,F1,F2,BLK} ;6 JOINTS & 2 FINGERS.
01900 DECLARE{J1NEW,J2NEW,J3NEW,J4NEW,J5NEW,J6NEW,J7NEW}
02000 DECLARE{J1OLD,J2OLD,J3OLD,J4OLD,J5OLD,J6OLD,J7OLD}
02100
02200 DECLARE{DELTA,TRAN}
02300 DECLARE{TICK,TOCK,DELTIC}
00100 ;FETCH BODY BY PNAME.---------------------------------------------
00200 BFETCH: LAC 4,@ARG1↔AOS ARG1
00300 LAC 5,@ARG1 ;ASCII PNAME.
00400 LAC 1,WORLD
00500 CCW 1,1
00600 CAME 1,WORLD
00700 GO[CAME 4,-2(1)↔GO .-2
00800 CAME 5,-1(1)↔GO .-2↔POP1J]
00900 SETZ 1,↔POP1J
01000
01100 ;INPUT WORD TO AC0.-----------------------------------------------
01200 WORDIN: SOSG IBUF+2↔IN 2,0
01300 GO[ILDB 0,IBUF+1↔POP0J]
01400 STATO 2,1B22
01500 GO[FATAL(WORDIN)]
01600 POP P,0
01700 GO EOX
01800 BEND;2/21/73------------------------------------------------------
01900 END